#ifndef _SERVO_CURTIS_H__
#define _SERVO_CURTIS_H__

#include <motor/motor.h>
#include <encoder/encoder_factory.h>

#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/int16.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/u_int16.hpp>
#include <std_msgs/msg/u_int32.hpp>


namespace ros_canopen_driver {

struct PDO_CACHE_CURTIS
{ 
    std::atomic<int16_t>     actual_current{0};
    std::atomic<uint8_t>     error_code{0};
    std::atomic<uint8_t>     steer_error{0};
    std::atomic<int32_t>     actual_speed{0};
    std::atomic<uint32_t>    odometer{0};
    std::atomic<uint16_t>    temperature{0};
    std::atomic<uint8_t>     battery{0};
    std::atomic<int16_t>     steer_angle{0};
    std::atomic<double>      timestamp{0.0};
};

class ServoCurtis : public Motor
{
public:
    ServoCurtis(std::shared_ptr<rclcpp::Node> node, const std::string name);
    ~ServoCurtis();

    int Motor_Init();

    bool Motor_Enable(bool onFlag);

    bool Motor_InitParam();

    bool Motor_HomeMachine();

    bool Motor_InitVmode();

    bool Motor_InitPmode();

    bool Motor_SwitchMode(int8_t mode);

 
    void Motor_SetAcc(uint32_t acc);

    void Motor_SetDec(uint32_t dec);

    void Motor_SetPmodeVel(uint32_t vel);

    void Motor_SetTargetVelRad(double vel);

    void Motor_SetTargetVelEnc(int vel);

    void Motor_SetTargetPosRad(double pos);

    void Motor_SetTargetPosEnc(int pos);


    MotorState Motor_GetState();

    double Motor_GetCurrentVel();

    double Motor_GetCurrentPos();

    double Motor_GetCurrent();

    uint16_t Motor_GetStatusWord();

    uint16_t Motor_GetErrCode();

    int8_t Motor_GetMode();


    void Motor_Brake(bool on);

    void Motor_Shutdown();

    bool Motor_Halt();

    bool Motor_Recover();

    double Motor_GetTimestamp() { return pc_.timestamp;}


    std::string Motor_Err2userErr(const uint16_t& motorErr);

private:
    SERVOMOTOR_PORT m_servo_motor;
    int preTargetEnc;
    std::atomic<MotorState> motorState;
    struct ForkLiftParam fork_param;
    std::shared_ptr<encoder::Encoder> encoder;

    struct PDO_CACHE_CURTIS pc_;

    /*
    /can0/device3/set_alternating_fork_lift_acc    
    /can0/device3/set_fork_lift_down_speed         
    /can0/device3/set_unused1
    /can0/device3/set_alternating_fork_lift_dec    
    /can0/device3/set_fork_lift_up_speed           
    /can0/device3/set_wheel_acc
    /can0/device3/set_alternating_fork_lift_speed  
    /can0/device3/set_target_steer_angle           
    /can0/device3/set_wheel_dec
    /can0/device3/set_control_word                 
    /can0/device3/set_unused0                      
    /can0/device3/set_wheel_speed
    */

    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_alternating_fork_lift_acc_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_alternating_fork_lift_dec_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt16>> set_alternating_fork_lift_speed_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt16>> set_fork_lift_enable_pub; // 24.03.08 haibo
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_fork_lift_down_speed_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_fork_lift_up_speed_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_wheel_acc_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_wheel_dec_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int16>> set_target_steer_angle_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::UInt8>> set_control_word_pub;
    std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Int16>> set_wheel_speed_pub;

    /*
    /can0/device3/get_actual_current  
    /can0/device3/get_error_code      
    /can0/device3/get_steer_error
    /can0/device3/get_actual_speed    
    /can0/device3/get_odometer        
    /can0/device3/get_temperature
    /can0/device3/get_battery         
    /can0/device3/get_steer_angle     
    /can0/device3/get_unused2
    */

    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr actual_current_sub;
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr error_code_sub;
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr steer_error_sub;
    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr actual_speed_sub;
    rclcpp::Subscription<std_msgs::msg::UInt32>::SharedPtr odometer_sub;
    rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr temperature_sub;
    rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr battery_sub;
    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr steer_angle_sub;

    void actual_current_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg);
    void error_code_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg);    
    void steer_error_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg);   
    void actual_speed_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg);  
    void odometer_sub_callback(const std_msgs::msg::UInt32::ConstSharedPtr& msg);      
    void temperature_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg);  
    void battery_sub_callback(const std_msgs::msg::UInt8::ConstSharedPtr& msg);       
    void steer_angle_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg);   

    int servo_encoderGet();

    void servo_checkForkStatus();

    bool servo_setForkVel(const int vel);

    void servo_doForkLift(bool up, int vel = 0);

};

}

#endif
